// This file is part of MatrixPilot.
//
//    http://code.google.com/p/gentlenav/
//
// Copyright 2009, 2010 MatrixPilot Team
// See the AUTHORS.TXT file for a list of authors of MatrixPilot.
//
// MatrixPilot is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MatrixPilot is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MatrixPilot.  If not, see <http://www.gnu.org/licenses/>.


// SEE END OF FILE FOR LICENSE TERMS

#include "libDCM_internal.h"
#include "libUDB_defines.h"

//		These are the routines for maintaining a direction cosine matrix
//		that can be used to transform vectors between the earth and plane
//		coordinate systems. The 9 direction cosines in the matrix completely
//		define the orientation of the plane with respect to the earth.
//		The inverse of the matrix is equal to its transpose. This defines
//		the so-called orthogonality conditions, which impose 6 constraints on
//		the 9 elements of the matrix.

//	All numbers are stored in 2.14 format.
//	Vector and matrix libraries work in 1.15 format.
//	This combination allows values of matrix elements between -2 and +2.
//	Multiplication produces results scaled by 1/2.


#define FILTERSHIFT 3
#define RMAX   0b0100000000000000	//	1.0 in 2.14 fractional format
#define GRAVITY ((long)(5280.0/SCALEACCEL))  // gravity in AtoD/2 units

//fractional ggain = GGAIN ;


//	rmat is the matrix of direction cosines relating
//	the body and earth coordinate systems.
//	The columns of rmat are the axis vectors of the plane,
//	as measured in the earth reference frame.
//	rmat is initialized to the identity matrix in 2.14 fractional format
fractional rmat[] = { RMAX , 0 , 0 , 0 , RMAX , 0 , 0 , 0 , RMAX } ;

//	rup is the rotational update matrix.
//	At each time step, the new rmat is equal to the old one, multiplied by rup.
fractional rup[] = { RMAX , 0 , 0 , 0 , RMAX , 0 , 0 , 0 , RMAX } ;

//	gyro rotation vector:
fractional omegagyro[] = { 0 , 0 , 0 } ;
fractional omega[] = { 0 , 0 , 0 } ;

//	gyro correction vectors:
fractional omegacorrP[] = { 0 , 0 , 0 } ;
fractional omegacorrI[] = { 0 , 0 , 0 } ;

//  acceleration, as measured in GPS earth coordinate system
fractional accelEarth[] = { 0 , 0 , 0 } ;

//	correction vector integrators ;
union longww CorrectionIntegral[] =  { { 0 } , { 0 } ,  { 0 } } ;

//	accumulator for computing adjusted omega:
fractional omegaAccum[] = { 0 , 0 , 0 } ;

//	gravity, as measured in plane coordinate system
fractional gplane[] = { 0 , 0 , GRAVITY } ;

//	horizontal velocity over ground, as measured by GPS (Vz = 0 )
fractional dirovergndHGPS[] = { 0 , RMAX/2 , 0 } ;

//	horizontal direction over ground, as indicated by Rmatrix
fractional dirovergndHRmat[] = { 0 , RMAX/2 , 0 } ;

//	rotation angle equal to omega times integration factor:
fractional theta[] = { 0 , 0 , 0 } ;

//	matrix buffer:
fractional rbuff[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ;

//	vector buffer
fractional errorRP[] = { 0 , 0 , 0 } ;
fractional errorYawground[] = { 0 , 0 , 0 } ;
fractional errorYawplane[]  = { 0 , 0 , 0 } ;

//	measure of error in orthogonality, used for debugging purposes:
fractional error = 0 ;

fractional declinationVector[2] ;


int vref_adj ;


#define ACCEL2DELTAV ((.025*GRAVITYM*4.0*RMAX)/GRAVITY)
#define VELOCITY2LOCATION (0.025*.01*4.0*RMAX)

#define VELTAU 2.0

#define TIMESTEP 0.025
#define MAX16 (4.0*RMAX)

#define KPVELREAL (2.0/VELTAU)
#define KIVELREAL (1.0/(VELTAU*VELTAU))

#define KPVELOCITY (TIMESTEP*MAX16*16.0*KPVELREAL)
#define KIVELOCITY (TIMESTEP*TIMESTEP*MAX16*256.0*KIVELREAL)

#define KPLOCATION (TIMESTEP*MAX16*8.0*KPVELREAL)
#define KILOCATION (TIMESTEP*TIMESTEP*MAX16*128.0*KIVELREAL)

//      velocity, as estimated by the IMU
union longww IMUvelocityx =  { 0 }  ;
union longww IMUvelocityy =  { 0 }  ;
union longww IMUvelocityz =  { 0 }  ;

//      velocity error in the IMU feedback loop 
fractional velocityErrorBody[] = { 0 , 0 , 0 } ;
fractional locationErrorEarth[] = { 0 , 0 , 0 } ;

//      IMU velocity correction vectors:
int velocityCorrP[] = { 0 , 0 , 0 } ;
int velocityCorrI[] = { 0 , 0 , 0 } ;

//      location, as estimated by the IMU
union longww IMUlocationx =  { 0 }  ;
union longww IMUlocationy =  { 0 }  ;
union longww IMUlocationz =  { 0 }  ;


//      IMU location correction vectors:
int locationCorrP[] = { 0 , 0 , 0 } ;
int locationCorrI[] = { 0 , 0 , 0 } ;

//      correction velocity vector integrators ;
union longww velocityCorrectionIntegral[] =  { { 0 } , { 0 } ,  { 0 } } ;

//      correction position vector integrators ;
union longww locationCorrectionIntegral[] =  { { 0 } , { 0 } ,  { 0 } } ;


fractional magFieldEarth[3] ;

extern fractional udb_magFieldBody[3] ;
extern fractional udb_magOffset[3] ;

fractional magFieldEarthPrevious[3] ;
fractional magFieldBodyPrevious[3] ;

fractional rmatPrevious[9] ;

int offsetDelta[3] ;


/****************************************************************************/
// This is part of the servo and radio interface software
//
// ServoInterface source code
//	http://code.google.com/p/rc-servo-interface
//
// Copyright 2010 ServoInterface Team
// See the AUTHORS.TXT file for a list of authors of ServoInterface.
//
// ServoInterface is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// ServoInterface is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License 
// along with ServoInterface.  If not, see <http://www.gnu.org/licenses/>.
//
// Many parts of ServoInterface use either modified or unmodified code
// from the MatrixPilot pilot project.
// The project also contains code for modifying MatrixPilot to operate
// with ServoInterface.
// For details, credits and licenses of MatrixPilot see the AUTHORS.TXT file.
// or see this website: http://code.google.com/p/gentlenav
/****************************************************************************/
